#include "ConnectionThread.h"
#include "Communication.h"
#include <csignal>
#include <QtEndian>

ConnectionThread::ConnectionThread(QString host, AiboState* state, QObject *parent) :
    QThread(parent)
{
    m_host = host;

    m_aiboState = state;

    m_tcpConnected = false;
    m_walkConnected = false;
    m_headConnected = false;
    m_behaviorConnected = false;
    m_jointMonitorConnected = false;
    m_cameraConnected = false;

	m_cmd = false;
	m_move = false;
	m_head = false;

    m_menuCmdClient = NULL;
    m_walkCmdClient = NULL;
    m_headCmdClient = NULL;
    m_behaviorCmdClient = NULL;
    m_jointMonitorClient = NULL;

    moveToThread(this);
}

void ConnectionThread::startBehavior()
{
    if (m_behaviorCmdClient) {
        QByteArray msg("start\n");
        m_behaviorCmdClient->write( qToLittleEndian( msg ), msg.length());
        m_behaviorCmdClient->flush();
		qDebug() << "Behavior Started";
    }
}

void ConnectionThread::walkForward()
{
    if (m_walkCmdClient) {
        QByteArray msg("FWD");
        m_walkCmdClient->write( qToLittleEndian( msg ), msg.length());
        m_walkCmdClient->flush();
    }
}

void ConnectionThread::stopForward()
{
    if (m_walkCmdClient) {
        QByteArray msg("STP");
		m_jointMonitorClient->write( qToLittleEndian( msg ), msg.length());
        m_jointMonitorClient->flush();
    }
}

void ConnectionThread::stopBehavior()
{
    if (m_behaviorCmdClient) {
        QByteArray msg("stop\n");
        m_behaviorCmdClient->write( qToLittleEndian( msg ), msg.length());
        m_behaviorCmdClient->flush();
    }
}

void ConnectionThread::closeConnection()
{
    if (m_menuCmdClient) {
        if (m_walkConnected) {
            QByteArray sendBytes(TEKKOTSUMON_WALK_REMOTE_CONTROL);
            m_menuCmdClient->write( qToLittleEndian( sendBytes ), sendBytes.length());
            m_menuCmdClient->flush();

            if (m_walkCmdClient) {
                m_walkCmdClient->close();
            }
			m_walkConnected = false;
			qDebug() << "Walk Ended";
            msleep(300);
        }

        if (m_headConnected) {
            QByteArray sendBytes(TEKKOTSUMON_HEAD_REMOTE_CONTROL);
            m_menuCmdClient->write( qToLittleEndian( sendBytes ), sendBytes.length());
            m_menuCmdClient->flush();

            if (m_headCmdClient) {
                m_headCmdClient->close();
            }
			m_headConnected = false;
            msleep(300);
        }

        if (m_behaviorConnected) {
            if (m_behaviorCmdClient) {
				this->stopBehavior();
                m_behaviorCmdClient->close();
            }
			m_behaviorConnected = false;
            msleep(300);
        }

        if (m_jointMonitorConnected) {
            QByteArray sendByte(TEKKOTSUMON_JOINT_MONITOR);
            m_menuCmdClient->write( qToLittleEndian( sendByte ), sendByte.length());
            m_menuCmdClient->flush();

            if (m_jointMonitorClient) {
                m_jointMonitorClient->close();
            }
			m_jointMonitorConnected = false;
            msleep(300);
        }

        if (m_cameraConnected) {
            QByteArray sendByte(CMD_TOGGLE_VISION);
            m_menuCmdClient->write( qToLittleEndian( sendByte ), sendByte.length());
            m_menuCmdClient->flush();

            if (m_UDPCamera) {
                m_UDPCamera->close();
            }
            m_cameraConnected = false;
            msleep(300);
        }

        m_tcpConnected = false;

        exit();
    }
}

void ConnectionThread::readMenu()
{

}

void ConnectionThread::readWalk()
{

}

void ConnectionThread::readHead()
{

}

void ConnectionThread::readBehavior()
{

}

void ConnectionThread::readJoint()
{

}

void ConnectionThread::readCamera()
{
    quint64 data_size = m_UDPCamera->pendingDatagramSize();
    if (m_UDPCamera->hasPendingDatagrams()) {
        quint64 read = 0;
        QByteArray recData = QByteArray(data_size, 0);
        while (read < data_size) {
            read += m_UDPCamera->read(recData.data() + read, data_size);
        }
        QByteArray converted = qFromLittleEndian( recData );
        parsePacket( converted );
    }
}

void ConnectionThread::displaySocketError(QAbstractSocket::SocketError error)
{
    qDebug() << "Socket Error: " << error;
}

void ConnectionThread::run()
{
	this->alive = true;
    initCommunication();
    initBehavior();
    initRemoteWalkControl();
    initRemoteHeadControl();
//    initJointMonitor();
    initCamera();

/*    if (m_tcpConnected) {
        sendCmd(CMD_TOGGLE_VISION);
        msleep(500);
        m_cameraThread->start();
    }
*/
    while( isAlive() ) {
        checkWalkCommandReady();
        checkHeadCommandReady();
	}

	this->closeConnection();
}

bool ConnectionThread::isAlive() {
	return this->alive;
}

void ConnectionThread::killConnection() {
	this->alive = false;
}

void ConnectionThread::requestSendCmd(const QByteArray &cmd)
{
	m_cmdData = cmd;
	m_cmd = true;
}

void ConnectionThread::requestSendWalkCmd(char type, float amount)
{
	this->m_moveType = type;
	this->m_moveAmount = amount;
	this->m_move = true;
}

void ConnectionThread::requestSendHeadCmd(char type, float amount)
{
	m_headType = type;
	m_headAmount = amount;
	m_head = true;
}

void ConnectionThread::initCommunication()
{
    int tries = 0;
	m_tcpConnected = false;
    while ( (tries < MAX_TRIES) && !m_tcpConnected) {
        m_menuCmdClient = new QTcpSocket(this);
		m_menuCmdClient->flush();
        connect(m_menuCmdClient, SIGNAL(readyRead()), this, SLOT(readMenu()));
        connect(m_menuCmdClient, SIGNAL(error(QAbstractSocket::SocketError)),
                this, SLOT(displaySocketError(QAbstractSocket::SocketError)));
        m_menuCmdClient->connectToHost(m_host, (quint16)AIBO_MENU_COMMAND_PORT);

        if (!m_menuCmdClient->waitForConnected(1000)) {
            qDebug() << "Error when trying to connect to Aibo on port" << AIBO_MENU_COMMAND_PORT;
            tries++;
        } else {
            qDebug() << "Tcp Connection OK.";
            m_tcpConnected = true;
        }
		m_menuCmdClient->flush();
        msleep(500);
    }
}

void ConnectionThread::initRemoteWalkControl()
{
    int tries = 0;
	m_walkConnected = false;
    if (m_tcpConnected) {
        while ((tries < MAX_TRIES) && !m_walkConnected) {
            sendCmd(TEKKOTSUMON_WALK_REMOTE_CONTROL);

            m_walkCmdClient = new QTcpSocket(this);
            connect(m_walkCmdClient, SIGNAL(readyRead()), this, SLOT(readWalk()));
            connect(m_walkCmdClient, SIGNAL(error(QAbstractSocket::SocketError)),
                    this, SLOT(displaySocketError(QAbstractSocket::SocketError)));
            m_walkCmdClient->connectToHost(m_host, (quint16)AIBO_WALKING_COMMAND_PORT);

            if (!m_walkCmdClient->waitForConnected(1000)) {
                qDebug() << "Error when trying to connect to Aibo on port: " << AIBO_WALKING_COMMAND_PORT;
                tries++;
            } else {
                qDebug() << "Walk Connection OK.";
                m_walkConnected = true;
            }
			m_walkCmdClient->flush();
            msleep(500);
        }
    } else {
        m_walkConnected = false;
    }
}

void ConnectionThread::initRemoteHeadControl()
{
    int tries = 0;
	m_headConnected = false;
    if (m_tcpConnected) {
        while ((tries < MAX_TRIES) && !m_headConnected) {
            sendCmd(TEKKOTSUMON_HEAD_REMOTE_CONTROL);

            m_headCmdClient = new QTcpSocket(this);
            connect(m_headCmdClient, SIGNAL(readyRead()), this, SLOT(readHead()));
            connect(m_headCmdClient, SIGNAL(error(QAbstractSocket::SocketError)),
                    this, SLOT(displaySocketError(QAbstractSocket::SocketError)));
            m_headCmdClient->connectToHost(m_host, (quint16)AIBO_HEADPOINT_COMMAND_PORT);

            if (!m_headCmdClient->waitForConnected(1000)) {
                qDebug() << "Error when trying to connect to Aibo on port" << AIBO_HEADPOINT_COMMAND_PORT;
                tries++;
            } else {
                qDebug() << "Head Connection OK.";
                m_headConnected = true;
            }
			m_headCmdClient->flush();
            msleep(500);
        }
    } else {
        m_headConnected = false;
    }
}

void ConnectionThread::initBehavior()
{
    int tries = 0;
	m_behaviorConnected = false;
    if (m_tcpConnected) {

        while ((tries < MAX_TRIES) && !m_behaviorConnected) {
            m_behaviorCmdClient = new QTcpSocket(this);
            connect(m_behaviorCmdClient, SIGNAL(readyRead()), this, SLOT(readBehavior()));
            connect(m_behaviorCmdClient, SIGNAL(error(QAbstractSocket::SocketError)),
                    this, SLOT(displaySocketError(QAbstractSocket::SocketError)));
            m_behaviorCmdClient->connectToHost(m_host, (quint16)AIBO_TOGGLING_BEHAVIOR_PORT);

            if (!m_behaviorCmdClient->waitForConnected(1000)) {
                qDebug() << "Error when trying to connect to Aibo on port" << AIBO_TOGGLING_BEHAVIOR_PORT;
                tries++;
            } else {
                qDebug() << "Behavior Connection OK.";
                m_behaviorConnected = true;
            }
			m_behaviorCmdClient->flush();
			this->startBehavior();
            msleep(500);
        }
    } else {
        m_behaviorConnected = false;
    }
}

void ConnectionThread::initJointMonitor()
{
    int tries = 0;
	m_jointMonitorConnected = false;
    if (m_tcpConnected) {
        while ((tries < MAX_TRIES) && !m_jointMonitorConnected) {
			sendCmd( QByteArray(TEKKOTSUMON_JOINT_MONITOR) );

            m_jointMonitorClient = new QTcpSocket(this);
            connect(m_jointMonitorClient, SIGNAL(readyRead()), this, SLOT(readJoint()));
            connect(m_jointMonitorClient, SIGNAL(error(QAbstractSocket::SocketError)),
                    this, SLOT(displaySocketError(QAbstractSocket::SocketError)));
            m_jointMonitorClient->connectToHost(m_host, (quint16)AIBO_JOINT_MONITOR_PORT);

            if (!m_jointMonitorClient->waitForConnected(1000)) {
                qDebug() << "Error when trying to connect to Aibo on port" << AIBO_JOINT_MONITOR_PORT;
                tries++;
            } else {
                qDebug() << "Joint Connection OK.";
                m_jointMonitorConnected = true;
            }
			m_jointMonitorClient->flush();
            msleep(500);
        }
    } else {
        m_jointMonitorConnected = false;
    }
}

void ConnectionThread::initCamera()
{
    int tries = 0;
    m_cameraConnected = false;

    if (m_tcpConnected) {
        m_UDPCamera = new QUdpSocket(this);
        connect(m_UDPCamera, SIGNAL(readyRead()), this, SLOT(readCamera()));
        connect(m_UDPCamera, SIGNAL(error(QAbstractSocket::SocketError)),
                this, SLOT(displaySocketError(QAbstractSocket::SocketError)));

        while ( (tries < MAX_TRIES) && !m_cameraConnected) {
            sendCmd(CMD_TOGGLE_VISION);
            msleep(500);
            m_UDPCamera->connectToHost(m_host, (quint16)AIBO_CAMERA_PORT);
            //bool is_bound = m_UDPCamera->bind(QHostAddress::LocalHost, (quint16)AIBO_CAMERA_PORT, QUdpSocket::ShareAddress);

            if ( m_UDPCamera->waitForConnected(1000) ) {
                qDebug() << "Camera connection OK.";
                m_cameraConnected = true;
                m_UDPCamera->write(qToLittleEndian(QByteArray("connection request")));
            } else {
                qDebug() << "Camera connection failed.";
                tries ++;
            }
            msleep(500);
        }
    }
}

void ConnectionThread::checkWalkCommandReady()
{
    if (m_aiboState->isWalkInstructionOn()) {
        if (m_aiboState->isForward()) {
            sendWalkCmd(CMD_WALK_FORWARD, m_aiboState->getForwardSpeed());
            m_aiboState->stopForward();
			qDebug() << "ConnectionThread: Walk Forward Handled - " << m_aiboState->getForwardSpeed() << endl;
        } else if (m_aiboState->isStrafe()) {
            sendWalkCmd(CMD_WALK_STRAFE, m_aiboState->getStrafeSpeed());
            m_aiboState->stopStrafe();
			qDebug() << "ConnectionThread: Walk Strafe Handled - " << m_aiboState->getStrafeSpeed() << endl;
        } else if (m_aiboState->isTurning()) {
            sendWalkCmd(CMD_WALK_ROTATE, m_aiboState->getTurnSpeed());
            m_aiboState->stopTurning();
			qDebug() << "ConnectionThread: Walk Turn Handled - " << m_aiboState->getTurnSpeed() << endl;
        }
    }
}

void ConnectionThread::checkHeadCommandReady()
{
    if (m_aiboState->isHeadInstructionOn()) {
        if (m_aiboState->isPanning()) {
            sendHeadPointCmd(CMD_HEADPOINT_PAN, m_aiboState->getPan());
            m_aiboState->stopPan();
			qDebug() << "ConnectionThread: Head Pan Handled - " << m_aiboState->getPan() << endl;
        } else if (m_aiboState->isRolling()) {
            sendHeadPointCmd(CMD_HEADPOINT_ROLL, m_aiboState->getRoll());
            m_aiboState->stopRoll();
			qDebug() << "ConnectionThread: Head Roll Handled - " << m_aiboState->getRoll() << endl;
        } else if (m_aiboState->isTilting()) {
            sendHeadPointCmd(CMD_HEADPOINT_TILT, m_aiboState->getTilt());
            m_aiboState->stopTilt();
			qDebug() << "ConnectionThread: Head Tilt Handled - " << m_aiboState->getTilt() << endl;
        }
    }
}

void ConnectionThread::parsePacket(const QByteArray &packet)
{
    int index = 0;
    AIBOPacket ap;

    ap.nameLen = (uint)(packet.mid(index, 1).data())[0];
    index += 4;
    ap.name = QString(packet.mid(index, ap.nameLen).data());
    index += ap.nameLen + 1;
    ap.imageEncoding = (uint)(packet.mid(index, 1).data())[0];
    index += 4;
    ap.imageCompression = (uint)(packet.mid(index, 1).data())[0];
    index += 4;
    ap.imageWidth = (uint)(packet.mid(index, 1).data())[0];
    m_width = ap.imageWidth;
//    qDebug() << "width" << m_width;
    index += 4;
    ap.imageHeight = (uint)(packet.mid(index, 1).data())[0];
    m_height = ap.imageHeight;
//    qDebug() << "height" << m_height;
    index += 4;
    ap.imageTimestamp = packet.mid(index, 4).toUInt();
    index += 4;
    ap.imageFramenumber = packet.mid(index, 4).toUInt();
    index += 4;

    ap.generatorNameLen = (uint)(packet.mid(index, 1).data())[0];
    index += 4;
    ap.generatorName = QString(packet.mid(index, ap.generatorNameLen).data());
    index += ap.generatorNameLen + 1;

    ap.specificGenNameLen = (uint)(packet.mid(index, 1).data())[0];
    index += 4;
    ap.specificGenName = QString(packet.mid(index, ap.specificGenNameLen).data());
    index += ap.specificGenNameLen + 1;

    ap.jpegImageDataSize = (uint)(packet.mid(index, 1).data())[0];
    index += 4;

    index = 89; // hack
    m_currImageData = packet.mid(index, packet.length() - index);
}

void ConnectionThread::sendCmd( const QByteArray &cmd)
{
	m_menuCmdClient->write( qToLittleEndian( cmd ), cmd.length());
    m_menuCmdClient->flush();
    m_cmd = false;
}

void ConnectionThread::sendWalkCmd(char type, float amount)
{
    if (m_walkCmdClient) {
        QByteArray sendBytes = QByteArray(5, 0);
        sendBytes[0] = type;
        QByteArray floatB(reinterpret_cast<const char*>(&amount), sizeof(amount));

        for (int i = 0; i < floatB.length(); i++) {
            sendBytes[i + 1] = floatB[i];
        }

        m_walkCmdClient->write( qToLittleEndian( sendBytes ), sendBytes.length());
        m_walkCmdClient->flush();

    }
	this->m_move = false;
}

void ConnectionThread::sendHeadPointCmd(char type, float amount)
{
    if (m_headCmdClient) {
        QByteArray sendBytes = QByteArray(5, 0);
        sendBytes[0] = type;
        QByteArray floatB(reinterpret_cast<const char*>(&amount), sizeof(amount));

        for (int i = 0; i < floatB.length(); i++) {
            sendBytes[i + 1] = floatB[i];
        }

        m_headCmdClient->write( qToLittleEndian( sendBytes ), sendBytes.length());
        m_headCmdClient->flush();
    }
    m_head = false;
}

bool ConnectionThread::isTcpConnected()
{
	return m_tcpConnected;
}

bool ConnectionThread::isWalkConnected()
{
    return m_walkConnected;
}

bool ConnectionThread::isHeadConnected()
{
    return m_headConnected;
}

bool ConnectionThread::isBehaviorConnected()
{
    return m_behaviorConnected;
}

bool ConnectionThread::isJointMonitorConnected()
{
    return m_jointMonitorConnected;
}

bool ConnectionThread::isCameraConnected()
{
    return m_cameraConnected;
}

QByteArray ConnectionThread::getCurrImageData()
{
    readCamera();
    return m_currImageData;
}

int ConnectionThread::getCameraWidth()
{
    return m_width;
}

int ConnectionThread::getCameraHeight()
{
    return m_height;
}
